"""
公司：广州吕顺医用机器人有限公司
项目：登机箱 
名称：登机箱
版本：1.5 开发板
系统版本：MicroPython v1.19.1 on 2022-06-18
作者：Tiger Pang
修改时间：2023-12-12

Corporation：Guangzhou Lvshun Medical Robot Co., Ltd
Item:Electric luggage compartment
name:Electric luggage compartment
Version: V1.5 Development Board
System Version: MicroPython v1.19.1 on 2022-06-18
Author:Tiger Pang
Update:2023-12-12
"""

'''
更新日志：注释说明更新，加入WDT喂狗,灯效用线程运行，不占用主进程，同一时间只能一个播放一个灯效
'''


from pid import PID
from buzzer import Buzzer
from rgb import RGB
from handgrip import Handgrip
from ble import ESP32_BLE
import sys
import network
import ubinascii
import machine
import micropython
import uos
import ujson
import esp
import time
import _thread
import os
import ble
from machine import ADC, Pin, DAC, Timer, WDT
HIGHT_SPEED_MODE, NORMAL_MODE, SLOW_MODE = 240000000, 160000000, 80000000
machine.freq(NORMAL_MODE)
print("CPU频率:", int(machine.freq() / 1000000), "MHz")
print("存储方式:", sys.byteorder)
print("Flash:", int(esp.flash_size() / 1024 / 1024), "M")
micropython.mem_info()
print("系统信息:", os.uname())

# ============================================================================== #


"""
握把初始化，左握把引脚 36，右握把引脚 39
"""


left, right = Handgrip(36), Handgrip(39)  # 左右把手的实例化
initial_left_data = 0  # 左握把的初始化值
initial_right_data = 0  # 右握把的初始化值
set_flash_flag = 1  # 是否写入握把值标志位
hand_allow = 1  # 有无检测到握把数值     有则可以控制电机，没有则不能控制电机

# ============================================================================== #


"""
电机初始化，DAC控制速度，引脚 26,
方向控制引脚 33，1 --> 正转,0 --> 反转，
刹车引脚 32，1 -- > 不刹车，0 --> 刹车
驱动器电源开关引脚 12 0 --> 关闭电源 1 --> 开启电源
"""


motor_dac = DAC(Pin(26))
motor_dac.write(0)


motor_direction = Pin(33, Pin.OUT)
motor_direction.value(1)

motor_brake = Pin(32, Pin.OUT)
motor_brake.value(0)
brake_strength = 50  # 刹车力度

power_switch = Pin(12, Pin.OUT)
power_switch.value(1)
dirver_start = time.ticks_ms()  # 电源关闭倒计时开始时间
dirver_end = time.ticks_ms()  # 电源关闭倒计时结束时间

first_start = 1

wdt = WDT(timeout=2000)  # enable it with a timeout of 2s

# ============================================================================== #

"""
RGB初始化，引脚 27，RGB数量 8
"""

rgb = RGB(27, 8)  # RGB灯的实例化

# ============================================================================== #


"""
蜂鸣器数值，引脚 14
"""


buzzer = Buzzer(14)  # 蜂鸣器的实例化

# ============================================================================== #


"""
蓝牙基本信息
"""


ble_name = "Connell-SL01-" + \
    ubinascii.hexlify(network.WLAN().config('mac'),
                      ':').decode()[-2:]  # 被检索时的名字
ble = ESP32_BLE(ble_name, rgb, buzzer)  # 被检索名称

# ============================================================================== #

"""
速度变量
"""

speed_num = 0  # 计算速度的数据上传蓝牙app

# ============================================================================== #

"""
PID初始化
"""

pid = PID(max=255, min=0, P=0.4, I=0.02, D=0.01,
          current_time=None, sample_interval=50)
speed = 0
last_speed = 0

# ============================================================================== #

"""
OTA的标志位置零，下次启动不OTA
"""


def Ota_Boot_flash():
    data = {
        "Ota_main": 0,
        "Ota_rgb": 0,
        "Ota_ble": 0,
        "Ota_handgrip": 0,
        "Ota_buzzer": 0,
        "Ota_pid": 0,
        "Ota_pwm": 0,
    }
    # 将数据转换为JSON格式
    json_data = ujson.dumps(data)
    # 已完成初次采样 标志位写入Flash
    try:
        with open("Ota_Boot_flash.json", "w") as f:
            f.write(json_data)
    except Exception as e:
        print("Ota_Boot_flash error", e)

# ============================================================================== #


"""
从Flash中读取RGB数据文件
"""


def check_flash_Rgb():
    try:
        with open("flash_rgb_data.json", "r") as f:
            json_data = f.read()
            # 将JSON数据转换为字典
            data = ujson.loads(json_data)
            # 读取变量的值
            rgb.mode = data["rgb_mode_flash_data"]
            rgb.light = data["rgb_light_flash_data"]
            rgb.last_light = rgb.light
            rgb.light_speed = data["rgb_speed_flash_data"]
            rgb.R = data["rgb_R_flash_data"]
            rgb.G = data["rgb_G_flash_data"]
            rgb.B = data["rgb_B_flash_data"]
            # 打印读取的数据
            # print("rgb模式数据:", rgb.mode)
            # print("rgb亮度数据:", rgb.light)
            # print("rgb灯速数据:", rgb.light_speed)
            # print("R: {}, G: {}, B: {}".format(rgb.R, rgb.G, rgb.B))
    except Exception as e:
        print("check_flash_Rgb error", e)

# ============================================================================== #


"""
从Flash中读取限速数据文件
"""


def check_flash_Speed():
    try:
        with open("max_speed_flash_data.json", "r") as f:
            json_data = f.read()
            # 将JSON数据转换为字典
            data = ujson.loads(json_data)
            # 读取变量的值
            ble.max_speed = data["max_speed_flash_data"]
            # 打印读取的数据
            # print("限速数据:", ble.max_speed)
    except Exception as e:
        print("check_flash_Speed error", e)

# ============================================================================== #


"""
从Flash中读取响铃数据，并判断是否要开机响铃
"""


def check_flash_Buzzer():
    try:
        with open("buzzer_flash_data.json", "r") as f:
            json_data = f.read()
            # 将JSON数据转换为字典
            data = ujson.loads(json_data)
            # 读取变量的值
            ble.buzzeropen = data["buzzer_open_data"]
            ble.buzzerblue = data["buzzer_blue_data"]
            if data["buzzer_open_data"] == 1:
                _thread.start_new_thread(buzzer.Buzzer_Play, ("buzzer1",))
            # 打印读取的数据
            # print("限速数据:", ble.buzzeropen)
    except Exception as e:
        print("check_flash_Buzzer error", e)

# ============================================================================== #


"""
检查Flash中是否存在握把数据文件,不存在则重新采样
"""


def check_flash():
    global set_flash_flag
    # 检查Flash中是否存在数据文件
    if "flash_data.txt" in uos.listdir():
        # 从Flash中读取变量
        with open("flash_data.txt", "r") as f:
            set_flash_flag = bool(f.read())
        print("读取到的flash值:", set_flash_flag, "不采取初次采样")
        flash_read()
    else:
        print("Flash中不存在数据文件")
        # 定义默认值
        set_flash_flag = 1
        # 将默认值保存到Flash
        with open("flash_data.txt", "w") as f:
            f.write(str(set_flash_flag))
        print("已保存默认值到Flash:", set_flash_flag)
        initialize_flash_int()
        flash_read()

# ============================================================================== #


"""
初始化阶段获取初始ADC数值并写入flash中=====(仅在更换握把时调用一次)
"""


def initialize_flash_int():
    global initial_left_data, initial_right_data
    global set_flash_flag, hand_allow
    samples = []
    num_samples = 300  # 采样次数
    print("frist time samples ....")
    for _ in range(num_samples):
        samples.append(left.ADC_Acquisition())
        time.sleep_ms(10)  # 采样间隔时间，根据实际情况调整
    # 去掉最大值和最小值
    samples = sorted(samples)[5:-5]
    # 计算平均值
    initial_left_data = sum(samples) // len(samples)

    samples = []
    for _ in range(num_samples):
        samples.append(right.ADC_Acquisition())
        time.sleep_ms(10)  # 采样间隔时间，根据实际情况调整
    # 去掉最大值和最小值
    samples = sorted(samples)[5:-5]
    # 计算平均值
    initial_right_data = sum(samples) // len(samples)
    print("samples success: left: {}, right: {}".format(
        initial_left_data, initial_right_data))
    if initial_left_data == 0:
        print("未检测到握把")
        hand_allow = 0
    elif initial_right_data == 0:
        print("未检测到握把")
        hand_allow = 0
    else:
        # 将变量保存到Flash
        data = {
            'right_flash_data': initial_right_data,
            'left_flash_data': initial_left_data
        }
        # 将数据转换为JSON格式
        json_data = ujson.dumps(data)
        # 已完成初次采样 标志位写入Flash
        with open('flash_data.json', 'w') as f:
            f.write(json_data)
        set_flash_flag = 0
        # 将默认值保存到Flash
        with open('flash_data.txt', 'w') as f:
            f.write(str(set_flash_flag))
        print('初次采样成功已保存默认值到Flash:', set_flash_flag)

# ============================================================================== #


"""
读取握把的数值
"""


def flash_read():
    global initial_right_data, initial_left_data
    global hand_allow
    # 从Flash中读取数据
    if hand_allow == 1:
        with open("flash_data.json", "r") as f:
            json_data = f.read()
            # 将JSON数据转换为字典
            data = ujson.loads(json_data)
            # 读取变量的值
            initial_right_data = data["right_flash_data"]
            initial_left_data = data["left_flash_data"]
            # 打印读取的数据
            print("右手握把数据:", initial_right_data)
            print("左手握把数据:", initial_left_data)

# ============================================================================== #


""""
电机控制函数，采集握把数值经过PID计算，输入数值到电机函数控制电机
"""


def Motor_Control():
    global speed_num, last_speed, speed
    try:
        # 获取当前握把的ADC数值
        left_data = left.ADC_Acquisition()
        right_data = right.ADC_Acquisition()
        # 计算与初始值的差异
        left_diff = abs(left_data - initial_left_data)
        right_diff = abs(right_data - initial_right_data)
        # 根据差异调整电机控制参数
        retreat = int(left_diff / 2150 * 255) if left_diff in range(0,
                                                                    2150) else 255  # else 0-255
        front = int(right_diff / 2150 * 255) if right_diff in range(0,
                                                                    2150) else 255  # else 0-255
    except Exception as e:
        print("Motor_Control get value error", e)
        # print(front,retreat)
        # print(ble.motor_flag)
    if ble.motor_flag == 0:
        try:
            if front > 50 and retreat < 10:  # 前进
                Motor_Speed(1, front)
            elif retreat > 50 and front < 10:  # 后退
                Motor_Speed(0, retreat)
            else:  # 停止
                Motor_Brake(retreat)
        except Exception as e:
            print("Motor_Control manual error", e)

    elif ble.motor_flag == 1:
        try:
            # print("welcome bule thread")
            if front > 50 or retreat > 50:
                ble.motor_flag = 0
            print(ble.motor_go)
            if ble.motor_go > 0 and ble.motor_back == 0:
                Motor_Speed(1, ble.motor_go)
                speed_num = int(ble.motor_go / 255 * 26)
            elif ble.motor_back > 0 and ble.motor_go == 0:
                Motor_Speed(0, ble.motor_back)
                speed_num = int(ble.motor_back / 255 * 26)
            elif ble.motor_go == 0 and ble.motor_back == 0:
                Motor_Brake(150)
            else:
                Motor_Brake(150)
                print("blue ctrl motor mode 1 error")
        except Exception as e:
            print("Motor_Control ble control error", e)
    wdt.feed()  # 喂狗
    time.sleep_ms(100)

# ============================================================================== #


'''
电机的速度方向控制，dir是控制前进或者后退，1 -- > 前进 0 --> 后退
speed 控制的速度，电机最低50启动效果较好，低于50顿挫感比较明显
'''


def Motor_Speed(dir, speed):
    global dirver_start, last_speed, speed_num, first_start
    dirver_start = time.ticks_ms()
    rgb.light = rgb.last_light
    power_switch.value(1)
    motor_brake.value(1)
    motor_direction.value(dir)
    pid.set_target(int(speed * int(ble.max_speed) / 255))
    speed = pid.update(last_speed, time.ticks_ms())
    speed = max(50, speed)
    last_speed = speed
    speed_num = int(speed/255)
    if first_start == 1:
        motor_dac.write(100)
        first_start = 0
        return
    motor_dac.write(speed)
    # print(dir,speed)

# ============================================================================== #


"""
电机刹车控制，strength 刹车的力度控制，0代表滑行，不刹住轮子。
"""


def Motor_Brake(brake_strength):
    # print("motor stop")
    global dirver_start, dirver_end, speed_num, first_start
    first_start = 1
    dirver_end = time.ticks_ms()
    delay = time.ticks_diff(dirver_end, dirver_start)
    if delay > 30000:
        power_switch.value(0)
        rgb.light = 0
    motor_brake.value(0)
    motor_dac.write(brake_strength)
    motor_brake.value(0)
    motor_direction.value(1)
    speed_num = 0


# ============================================================================== #


"""
定时器发送蓝牙数据。
"""


def Timer_Send_Data(timer):
    global speed_num
    if ble.bule_flag == 1:
        try:
            ble.send("speed+" + str(speed_num))
        except:
            print("data send error")
    elif ble.bule_flag == 0:
        pass
    # print('蓝牙未连接')
    else:
        print("蓝牙状态异常")

# ============================================================================== #


"""
蓝牙数据发送定时器创建并启动
"""

timer = Timer(0)
timer.init(period=500, mode=Timer.PERIODIC, callback=Timer_Send_Data)

# ============================================================================== #


'''
主函数启动函数
'''


def main():
    Ota_Boot_flash()
    check_flash_Buzzer()
    check_flash_Speed()
    check_flash_Rgb()
    _thread.start_new_thread(rgb.rgb_thread, ())
    global hand_allow
    check_flash()
    print("enter esp32 success")
    ble.motor_flag = 0
    if hand_allow == 0:
        print("未检测到握把,请检测后重试")
        time.sleep_ms(1000)
        machine.reset()
    if hand_allow == 1:
        while True:
            Motor_Control()


if __name__ == "__main__":
    main()
